#include "xen/physical/body.hpp"

namespace Xen::Physical {

        /************************
         * Body
         ***********************/
        Body::Body(const World& world) {
            id_ = dBodyCreate(world.id_);
        }

        Body::Body(const World& world, const glm::vec3& pos, const glm::vec3& rotation) {
            id_ = dBodyCreate(world.id_);
            SetPosition(pos);
            SetRotation(rotation);
        }

        Body::~Body() {
            dBodyDestroy(id_);
        }

        void Body::AttachGeom(const Ref<Geomentry>& g) {
            dGeomSetBody(g->id_, id_);
        }

        void Body::SetPosition(const glm::vec3& pos) {
            dBodySetPosition(id_, pos.x, pos.y, pos.z);
        }

        void Body::SetRotation(const glm::vec3& rotation) {
            // dBodySetRotation(id_, pos.x, pos.y, pos.z);
            // TODO not finish
            XEN_CORE_WARN("Body::SetRotation() not implement");
        }

        void Body::SetLinearVel(const glm::vec3& vel) {
            dBodySetLinearVel(id_, vel.x, vel.y, vel.z);
        }

        void Body::SetAngularVel(const glm::vec3& vel) {
            dBodySetAngularVel(id_, vel.x, vel.y, vel.z);
        }

        glm::vec3 Body::GetLinearVel() const {
            const dReal* v = dBodyGetLinearVel(id_);
            return glm::vec3(v[0], v[1], v[2]);
        }

        glm::vec3 Body::GetAngularVel() const {
            const dReal* v = dBodyGetAngularVel(id_);
            return glm::vec3(v[0], v[1], v[2]);
        }

        const glm::vec3 Body::GetPosition() {
            const dReal* pos = dBodyGetPosition(id_);
            return glm::vec3(pos[0], pos[1], pos[2]);
        }

        const glm::vec3 Body::GetRotation() {
            const dReal* rot = dBodyGetRotation(id_);
            return glm::vec3(rot[0], rot[1], rot[2]);
        }

        void Body::SetForce(const glm::vec3& f) {
            dBodySetForce(id_, f.x, f.y, f.z);
        }

        glm::vec3 Body::GetForce() const {
            const dReal* f = dBodyGetForce(id_);
            return glm::vec3(f[0], f[1], f[2]);
        }

        void Body::SetTorque(const glm::vec3& f) {
            dBodySetTorque(id_, f.x, f.y, f.z);
        }

        glm::vec3 Body::GetTorque() const {
            const dReal* f = dBodyGetTorque(id_);
            return glm::vec3(f[0], f[1], f[2]);
        }


        /************************
         * SphereBody
         ***********************/

        SphereBody::SphereBody(const World& world, float mass, const glm::vec3& position, float r)
        :Body(world), mass_(mass, r), r_(r)
        {
            SetPosition(position);
        }

        const Mass& SphereBody::GetMass() const {
            return mass_;
        }

        /************************
         * BoxBody
         ***********************/

        BoxBody::BoxBody(const World &world, float mass, const glm::vec3 &position, const glm::vec3 &l)
        :Body(world), mass_(mass, l), l_(l) {
            SetPosition(position);
        }

        const Mass &BoxBody::GetMass() const {
            return mass_;
        }
    }
